#include "opencv2/video/tracking.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/highgui/highgui.hpp"
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/nonfree/nonfree.hpp>
#include <cvctracker/cvc_tracker.h>
#include <iostream>
#include <ctype.h>

using namespace cv;
using namespace std;

Point2f pt;
bool addRemovePt = false;
void onMouse( int event, int x, int y, int /*flags*/, void* /*param*/ )
{
    if( event == CV_EVENT_LBUTTONDOWN )
    {
        pt = Point2f((float)x,(float)y);
        addRemovePt = true;
    }
}

int main( int argc, char** argv )
{
    VideoCapture cap;
    TermCriteria termcrit(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03);
    Size subPixWinSize(10,10), winSize(31,31);
    
    const int MAX_COUNT = 500;
    bool needToInit = false;
    bool nightMode = false;
    
    if( argc == 1 || (argc == 2 && strlen(argv[1]) == 1 && isdigit(argv[1][0])))
        cap.open(argc == 2 ? argv[1][0] - '0' : 0);
    else if( argc == 2 )
        cap.open(argv[1]);

    if( !cap.isOpened() )
    {
        cout << "Could not initialize capturing...\n";
        return 0;
    }

    
    namedWindow( "LK Demo", 1 );
    setMouseCallback( "LK Demo", onMouse, 0 );

	 Mat gray, prevGray, image;

	 cv::SurfFeatureDetector detector(400);
	 cv::SurfDescriptorExtractor extractor;
	 cv::DescriptorMatcher *matcher  = new cv::BFMatcher(cv::NORM_L2,true);
	 CVCTracker cvct(&detector , &extractor , matcher);

#if 1
	 cvct.loadMarkerImage("d:\\android16\\temp\\obidosRR.png");
#else
	 cv::Mat imageFile = cv::imread("d:\\android16\\temp\\obidosRR.png");
	 if (imageFile.data == NULL)
	 {
		 return 1;
	 }
	 cvct.loadMarkerImage(imageFile);
#endif
	
	 

	 for(;;)
    {
		 Mat frame;
        cap >> frame;
        if( frame.empty() )
            break;

        frame.copyTo(image);
        cvtColor(image, gray, CV_BGR2GRAY); 		
		
		//cvct.match(gray);

		if (cvct.matchChessboard( gray ))
		{
			cvct.calibrationCamera( gray );
			cvct.drawReprojection( gray, image );
		}

		Mat img_matches;
		//cvct.prepareResultImage(img_matches);

		//imshow("LK Demo", img_matches);
		imshow("LK Demo", image);

		  char c = (char)waitKey(10);
        if( c == 27 )
            break;
        switch( c )
        {
       /* case 'r':
            needToInit = true;
            break;
        case 'c':
            points[1].clear();
            break;
        case 'n':
            nightMode = !nightMode;
            break;*/
			case 'n':
        default:
            ;
        }
	}
}
